import rclpy
from example_ros2_interface.msg import RobotStatus
import math
from rclpy.node import Node
from time import sleep
from example_ros2_interface.srv import MoveRobot
class Robot():
    def __init__(self)->Node:
        self.current_pose = 0.0
        self.target_pose = 0.0
        self.status = RobotStatus.STATUS_STOP
        # super().__init__()
    def get_status(self):
        return self.status

    def get_current_pose(self):
        return self.current_pose

    def move_distance(self,distance):
        self.status = RobotStatus.STATUS_MOVEING
        self.target_pose += distance

        while math.fabs(self.target_pose - self.current_pose) > 0.01:
            step = distance / math.fabs(distance) * math.fabs(self.target_pose - self.current_pose) * 0.1 # 计算一步移动距离
            self.current_pose  += step 
            print(f"moved：{step} current_location：{self.current_pose}")
            sleep(0.5) 
        self.status_ = RobotStatus.STATUS_STOP 
        return self.current_pose

class Interface_robot(Node):
    def __init__(self,name):
        super().__init__(name)
        self.get_logger().info('running node:{}'.format(name))
        self.robot = Robot()
        self.move_robot_server = self.create_service(MoveRobot,"move_robot", self.handle_move_robot) 
        self.robot_status_publisher = self.create_publisher(RobotStatus,"robot_status", 10) 
        self.publisher_timer = self.create_timer(0.5, self.publisher_timer_callback)

    def publisher_timer_callback(self):

        msg = RobotStatus() 
        msg.status = self.robot.get_status()
        msg.pose = self.robot.get_current_pose()
        self.robot_status_publisher.publish(msg) 
        self.get_logger().info(f'publish current location：{msg.status} location：{msg.pose}')

    def handle_move_robot(self,request, response):
        self.robot.move_distance(request.distance)
        response.pose = self.robot.get_current_pose()
        return response
    
def main(args=None):
    rclpy.init(args=args)
    node = Interface_robot('itf_robot')
    rclpy.spin(node)
    rclpy.shutdown()